perm filename ARMCAL.SAI[LOU,BGB] blob
sn#006831 filedate 1974-12-08 generic text, type T, neo UTF8
00100 BEGIN "ARMCAL"
00200 REQUIRE "PREAMB.SAI[SYS,HE]" SOURCE_FILE;
00300 EXTERNAL PROCEDURE ARM_JOINT;
00400 REQUIRE -1 NEW_ITEMS;
00500 REQUIRE 200 STRING_SPACE;
00600 DEFINE $="GLOBAL";
00700 DEFINE CRLF="'15&'12";
00800 DEFINE ASSIGN="MATCH←FALSE;FOREACH";
00900 DEFINE HOLDS="DO IF MATCH THEN USERERR(0,0,""ASSIGN MULTIPLY DEFINED"")
01000 ELSE MATCH←TRUE;IF ¬MATCH THEN USERERR(0,0,""ASSIGN FAILS"")";
01100 BOOLEAN MATCH;
01200 REAL ROTAT;
01300 SAFE REAL ARRAY TP[0:15];
01400 SAFE REAL ARRAY TRANS[1:4,1:4];
01500 INTEGER I,J,MESS;
01600 BOOLEAN FRST_OPEN;
01700 BOOLEAN TEST;
01800 STRING S;
01900 INTEGER N;
02000 REAL TX,TY,TZ;
02100 INTEGER HAND;
02200 INTEGER NO;
02300 STRING FILE,DATFIL;
02400 DEFINE NUM_CELL="100";
02500 INTEGER ARRAY INDEX[1:NUM_CELL];
02600 SAFE REAL ARRAY RANGE[1:NUM_CELL,0:1];
02700 INTEGER FREE;
02800 INTEGER BREAK,EOF;
02900 INTEGER PTR1,PTR2,PTR3;
03000 SAFE REAL ARRAY TH,DTH,DIR[1:6];
03100 DEFINE MP="MESSAGE";
03200 REQUIRE "TRAJ.SAI[II,HE]" SOURCE_FILE;
03300 EXTERNAL PROCEDURE TRANSFORM(REAL ARRAY R,A,B);
03400 PRELOAD_WITH -2.5, -1.3, 15.0, PIBY2, -1.0, 0.0;
03500 SAFE REAL ARRAY V0[1:6];
03600 SAFE REAL ARRAY VA,VO[1:4];
03700 PRELOAD_WITH 0.0, 0.0, 1.0, 1.0;
03800 SAFE REAL ARRAY UZ[1:4];
03900 REAL ARRAY T1,T2[1:4,1:4];
04000 REAL ARRAY IP,V1,V2,O1,O2[1:4];
04100
04200 INTEGER PROCEDURE GET;
04300 BEGIN INTEGER P;
04400 P←FREE;
04500 FREE←INDEX[FREE];
04600 INDEX[P]←0;
04700 RANGE[P,0]←0.0;
04800 RANGE[P,1]←TPI;
04900 RETURN(P);
05000 END;
05100 STRING PROCEDURE PRINT(INTEGER I);
05200 BEGIN STRING S;
05300 IF ¬I THEN RETURN("()");
05400 PUSH_FORMAT(7,1);
05500 S←NULL;
05600 WHILE I DO BEGIN S←S&"("&CVF(RAD*RANGE[I,0])&CVF(RAD*RANGE[I,1])&")";
05700 I←INDEX[I];
05800 END;
05900 POP_FORMAT;
06000 RETURN(S);
06100 END;
06200
06300 PROCEDURE PVECT(STRING S;REAL ARRAY V);
06400 BEGIN INTEGER I;
06500 PUSH_FORMAT(6,2);
06600 OUTSTR(S);
06700 FOR I←1 STEP 1 UNTIL 4 DO OUTSTR(CVF(V[I]));
06800 OUTSTR(CRLF);
06900 POP_FORMAT;
07000 END;
07100
07200 PROCEDURE PMAT(STRING S;REAL ARRAY T);
07300 BEGIN INTEGER I,J;
07400 PUSH_FORMAT(6,2);
07500 OUTSTR(S&CRLF);
07600 FOR I←1 STEP 1 UNTIL 4 DO BEGIN
07700 FOR J←1 STEP 1 UNTIL 4 DO OUTSTR(CVF(T[I,J]));
07800 OUTSTR(CRLF);
07900 END;
08000 POP_FORMAT;
08100 END;
08200
08300
08400 PROCEDURE REPLACE(INTEGER L);
08500 IF L THEN BEGIN INTEGER H;
08600 H←L;
08700 WHILE INDEX[L]≠0 DO L←INDEX[L];
08800 INDEX[L]←FREE;
08900 FREE←H;
09000 END;
09100
09200 PROCEDURE RESET;
09300 BEGIN INTEGER I;
09400 FOR I←1 STEP 1 UNTIL NUM_CELL-1 DO INDEX[I]←I+1;
09500 INDEX[NUM_CELL]←0;
09600 FREE←1;
09700 END;
09800
09900 INTEGER PROCEDURE INTERSECT(INTEGER P1,P2;REAL S);
10000 BEGIN REAL MIN,MAX,R;
10100 INTEGER PR;
10200 IF(RANGE[P1,0]-(R←RANGE[P2,0]+S))*(R-RANGE[P1,1])≥0 THEN MIN←R-S
10300 ELSE IF(RANGE[P2,0]-(R←RANGE[P1,0]-S))*(R-RANGE[P2,1])≥0 THEN MIN←R
10400 ELSE RETURN(0);
10500 IF(RANGE[P1,0]-(R←RANGE[P2,1]+S))*(R-RANGE[P1,1])≥0 THEN MAX←R-S
10600 ELSE IF(RANGE[P2,0]-(R←RANGE[P1,1]-S))*(R-RANGE[P2,1])≥0 THEN MAX←R
10700 ELSE USERERR(0,0,"BAD RANGE ... INTERSECT");
10800 PR←GET;
10900 RANGE[PR,0]←IF MAX=TPI THEN MIN-TPI ELSE MIN;
11000 RANGE[PR,1]←IF MAX=TPI THEN 0.0 ELSE MAX;
11100 RETURN(PR);
11200 END;
11300
11400 INTEGER PROCEDURE INTERSECTION(INTEGER P1,P2);
11500 BEGIN INTEGER PR;
11600 IF(PR←INTERSECT(P1,P2,0)) THEN
11700 BEGIN IF ¬(INDEX[PR]←INTERSECT(P1,P2,TPI)) THEN INDEX[PR]←INTERSECT(P1,P2,-TPI) END ELSE
11800 IF ¬(PR←INTERSECT(P1,P2,TPI)) THEN PR←INTERSECT(P1,P2,-TPI);
11900 IF PR ∧ INDEX[PR] THEN BEGIN
12000 IF RANGE[PR,1]=RANGE[INDEX[PR],0] THEN
12100 BEGIN RANGE[PR,1]←RANGE[INDEX[PR],1];
12200 REPLACE (INDEX[PR]);
12300 INDEX[PR]←0;
12400 END ELSE IF RANGE[PR,0]=RANGE[INDEX[PR],1] THEN
12500 BEGIN RANGE[PR,0]←RANGE[INDEX[PR],0];
12600 REPLACE (INDEX[PR]);
12700 INDEX[PR]←0;
12800 END;END;
12900 RETURN (PR);
13000 END;
13100
13200 INTEGER PROCEDURE MERGE(INTEGER L1,L2);
13300 BEGIN INTEGER LS,LSA,PL,PR;
13400 PL←0;
13500 LSA←L1;
13600 WHILE L1 DO
13700 BEGIN LS←L2;
13800 WHILE LS DO
13900 BEGIN IF(PR←INTERSECTION(L1,LS)) THEN
14000 BEGIN IF INDEX[PR] THEN
14100 INDEX[INDEX[PR]]←PL ELSE
14200 INDEX[PR]←PL;
14300 PL←PR END;
14400 LS←INDEX[LS] END;
14500 L1←INDEX[L1];
14600 END;
14700 REPLACE(LSA);
14800 REPLACE(L2);
14900 RETURN (PL);
15000 END;
15100
15200 INTEGER PROCEDURE OVERLAP(INTEGER L2,L1;REAL SHIFT);
15300 BEGIN INTEGER LS,PL,PR;
15400 PL←0;
15500 LS←0;
15600 WHILE L2 DO BEGIN IF LS THEN LS←INDEX[LS]←GET ELSE LS←GET;
15700 RANGE[LS,0]←RANGE[L2,0]+SHIFT;
15800 RANGE[LS,1]←RANGE[L2,1]+SHIFT;
15900 L2←INDEX[L2];
16000 END;
16100 L2←LS;
16200 WHILE L1 DO
16300 BEGIN LS←L2;
16400 WHILE LS DO
16500 BEGIN IF(PR←INTERSECTION(L1,LS)) THEN
16600 BEGIN IF INDEX[PR] THEN
16700 INDEX[INDEX[PR]]←PL ELSE
16800 INDEX[PR]←PL;
16900 PL←PR END;
17000 LS←INDEX[LS] END;
17100 L1←INDEX[L1];
17200 END;
17300 REPLACE(L2);
17400 RETURN (PL);
17500 END;
17600
17700 REAL PROCEDURE TAN(REAL R);
17800 RETURN(SIN(R)/COS(R));
17900
18000 PROCEDURE PRINCIPAL(INTEGER P);
18100 BEGIN
18200 WHILE RANGE[P,0]>RANGE[P,1] DO RANGE[P,1]←RANGE[P,1]+TPI;
18300 WHILE RANGE[P,1]>RANGE[P,0]+TPI DO RANGE[P,1]←RANGE[P,1]-TPI;
18400 WHILE RANGE[P,1]>TPI DO
18500 BEGIN RANGE[P,0]←RANGE[P,0]-TPI;
18600 RANGE[P,1]←RANGE[P,1]-TPI;
18700 END;
18800 WHILE RANGE[P,0]≤-TPI DO
18900 BEGIN RANGE[P,0]←RANGE[P,0]+TPI;
19000 RANGE[P,1]←RANGE[P,1]+TPI;
19100 END;
19200 END;
19300
00100 INTEGER PROCEDURE TEN(SAFE REAL ARRAY TRANS);
00200 BEGIN INTEGER P1;
00300 REAL V1,W2,W1,J,T,F,A,M,B1,B2,TFM,C1,C2,SIGN;
00400 SAFE OWN REAL ARRAY P,O,W,VT1,VT2[1:4];
00500 PRELOAD_WITH 0,0,1,1;
00600 SAFE OWN REAL ARRAY K[1:4];
00700 DEFINE MIN="PI-1.7";
00800 DEFINE L="8.0",V2="L↑2+6.0↑2";
00900 COLVECT(P,TRANS,4);
01000 COLVECT(O,TRANS,2);
01100 V1←SQRT(V2);
01200 DIFFERENCE(W,P,SHOLDER);
01300 REDUCE(W);
01400 W2←DOT(W,W);
01500 W1←SQRT(W2);
01600 IF V1>W1+8.5 THEN RETURN(0);
01700 IF W1>V1+8.5 THEN RETURN(GET);
01800 J←ACOS((V2+8.5↑2-W2)/(2*8.5*V1));
01900 T←ASIN(8.5*SIN(J)/W1);
02000 F←PI-(J+T);
02100 MOVEV(VT1,O);
02200 VT1[3]←0.0;
02300 UNIT(VT1,VT1);
02400 MOVEV(VT2,W);
02500 VT2[3]←0.0;
02600 UNIT(VT2,VT2);
02700 A←ABS(ASIN(DOT(VT1,VT2)));
02800 M←ACOS(-W[3]/W1);
02900 IF(B1←(SIN(A)*SIN(M)/SIN(F)))<1.0 THEN
03000 B1←ASIN(B1) ELSE RETURN(GET);
03100 B2←PI-B1;
03200 IF A THEN BEGIN
03300 TFM←TAN((F+M)/2);
03400 C1←2*ATAN2(TFM*COS((A+B1)/2),COS((A-B1)/2));
03500 C2←2*ATAN2(TFM*COS((A+B2)/2),COS((A-B2)/2));
03600 END ELSE BEGIN
03700 C1←M+F;
03800 C2←M-F;
03900 END;
04000 P1←GET;
04100 RANGE[P1,0]←PIBY2;
04200 RANGE[P1,1]←PIBY2;
04300 CROSS(VT1,O,K);
04400 UNIT(VT1,VT1);
04500 SIGN←DOT(VT1,VT2);
04600 IF SIGN <0 THEN
04700 BEGIN RANGE[P1,0]←RANGE[P1,0]+C1;
04800 RANGE[P1,1]←RANGE[P1,1]+C2;
04900 END ELSE
05000 BEGIN RANGE[P1,0]←RANGE[P1,0]-C2;
05100 RANGE[P1,1]←RANGE[P1,1]-C1;
05200 END;
05300 PRINCIPAL(P1);
05400 RETURN (P1);
05500 END;
00100 INTEGER PROCEDURE TABLE(SAFE REAL ARRAY TRANS);
00200 BEGIN REAL H;
00300 INTEGER P;
00400 H←2.5-TRANS[3,4];
00500 IF H≥2.10 THEN RETURN(0);
00600 P←GET;
00700 IF H>-8.5 THEN
00800 BEGIN RANGE[P,0]←ASIN(H/8.5);
00900 RANGE[P,1]←PI-RANGE[P,0];
01000 END;
01100 RETURN(P);
01200 END;
01300 INTEGER PROCEDURE POST(SAFE REAL ARRAY TRANS);
01400 BEGIN INTEGER P1;
01500 REAL W2,W1,A,B,S;
01600 SAFE OWN REAL ARRAY P,O,W,VT1,VT2[1:4];
01700 PRELOAD_WITH 0,0,1,1;
01800 SAFE OWN REAL ARRAY K[1:4];
01900 COLVECT(P,TRANS,4);
02000 DIFFERENCE(W,P,SHOLDER);
02100 REDUCE(W);
02200 IF(W2←(W[1]↑2+W[2]↑2))<6.0↑2 THEN RETURN(0);
02300 W1←SQRT(W2);
02400 IF W1>6.0+8.75 THEN RETURN(GET);
02500 COLVECT(O,TRANS,2);
02600 MOVEV(VT1,O);
02700 VT1[3]←0.0;
02800 UNIT(VT1,VT1);
02900 MOVEV(VT2,W);
03000 VT2[3]←0.0;
03100 UNIT(VT2,VT2);
03200 B←ASIN(S←ABS(DOT(VT1,VT2)));
03300 A←(S*W1/6.0);
03400 IF A<1.0 THEN A←ASIN(A)-B ELSE RETURN(GET);
03500 S←SQRT(6.0↑2+W2-2*6.0*W1*COS(A));
03600 S←ACOS(S/8.75);
03700 P1←GET;
03800 CROSS(VT1,O,K);
03900 UNIT(VT1,VT1);
04000 IF DOT(VT1,VT2)<0 THEN RANGE[P1,0]←RANGE[P1,1]←PI ELSE
04100 RANGE[P1,0]←RANGE[P1,1]←0.0;
04200 RANGE[P1,0]←RANGE[P1,0]+S;
04300 RANGE[P1,1]←RANGE[P1,1]-S;
04400 PRINCIPAL(P1);
04500 RETURN(P1);
04600 END;
00100 BOOLEAN PROCEDURE POSSIBLE(SAFE REAL ARRAY T,J;REAL ROTAT);
00200 BEGIN
00300 EXTERNAL PROCEDURE MOVEV(REAL ARRAY V;REFERENCE REAL R);
00400 EXTERNAL PROCEDURE CROSS(REFERENCE REAL R,A,B);
00500 EXTERNAL PROCEDURE UNIT(REFERENCE REAL R,B);
00600 EXTERNAL PROCEDURE REDUCE(REFERENCE REAL R);
00700 SAFE REAL ARRAY V1,V2,V3[1:4];
00800 INTEGER I;
00900 T[4,1]←T[4,2]←T[4,3]←1.0;
01000 TRANSPOSE(T,T);
01100 T[3,1]←T[3,2]←0.0;
01200 T[3,3]←T[3,4]←1.0;
01300 CROSS(T[1,1],T[2,1],T[3,1]);
01400 UNIT(T[1,1],T[1,1]);
01500 MOVEV(V1,T[1,1]);
01600 MOVEV(V2,T[2,1]);
01700 ROTATE(V3,V1,V2,ROTAT);
01800 FOR I←1 STEP 1 UNTIL 4 DO T[3,I]←V3[I];
01900 CROSS(T[1,1],T[2,1],T[3,1]);
02000 REDUCE(T[1,1]);
02100 REDUCE(T[2,1]);
02200 REDUCE(T[3,1]);
02300 TRANSPOSE(T,T);
02400 T[4,1]←T[4,2]←T[4,3]←0.0;
02500 T[4,4]←1.0;
02600 ARM_SOLVE(T,J,I);
02700 RETURN(I);
02800 END;
02900
03000 INTEGER PROCEDURE LIMIT4(SAFE REAL ARRAY T;INTEGER P2);
03100 BEGIN REAL MID,R;
03200 SAFE REAL ARRAY J[1:6];
03300 REAL UL,LL;
03400 INTEGER P1;
03500 SAFE OWN REAL ARRAY P,O,W,VT1[1:4];
03600 PRELOAD_WITH 0,0,1,1;
03700 SAFE OWN REAL ARRAY K[1:4];
03800 IF (LL←RANGE[P2,0])=0 ∧ (UL←RANGE[P2,1])=TPI THEN BEGIN
03900 COLVECT(P,T,4);
04000 DIFFERENCE(W,P,SHOLDER);
04100 R←ATAN2(-W[3],SQRT(W[1]↑2+W[2]↑2));
04200 COLVECT(O,T,2);
04300 CROSS(VT1,O,K);
04400 R←IF DOT(VT1,W)>0 THEN R-PI ELSE -R;
04500 IF POSSIBLE(T,J,R) THEN RETURN(GET);
04600 R←R+PI;
04700 IF ¬POSSIBLE(T,J,R)THEN RETURN (0);
04800 UL←R+PI;
04900 LL←R-PI;
05000 MID←PI;
05100 END ELSE BEGIN
05200 IF POSSIBLE(T,J,(MID←(UL+LL)/2))THEN RETURN (GET);
05300 IF ¬POSSIBLE(T,J,UL)THEN RETURN(0);
05400 IF ¬POSSIBLE(T,J,LL)THEN RETURN(0);
05500 END;
05600 R←MID;
05700 WHILE R>0.02 DO IF POSSIBLE(T,J,(UL←UL-(R←R/2)))THEN UL←UL+R;
05800 R←MID;
05900 WHILE R>0.02 DO IF POSSIBLE(T,J,(LL←LL+(R←R/2)))THEN LL←LL-R;
06000 P1←GET;
06100 RANGE[P1,0]←LL+0.02;
06200 RANGE[P1,1]←UL-0.02;
06300 PRINCIPAL(P1);
06400 RETURN(P1);
06500 END;
06600
06700 INTEGER PROCEDURE ABLE(SAFE REAL ARRAY V,O,T);
06800 BEGIN INTEGER I;
06900 FOR I←1 STEP 1 UNTIL 4 DO BEGIN T[I,2]←O[I];T[I,4]←V[I] END;
07000 IF I←TEN(T)
07100 THEN IF I←MERGE(I,LIMIT4(T,I))
07200 THEN IF TRUE
07300 THEN IF I←MERGE(I,POST(T))
07400 THEN IF I←MERGE(I,TABLE(T))
07500 THEN RETURN(I)
07600 ELSE OUTSTR("TABLE INTERSECTION"&'15&'12)
07700 ELSE OUTSTR("POST INTERSECTION"&'15&'12)
07800 ELSE OUTSTR("JOINT 3 MAXIMUM"&'15&'12)
07900 ELSE OUTSTR("JOINT 4 STOP"&'15&'12)
08000 ELSE OUTSTR("JOINT 3 MINIMUN"&'15&'12);
08100 RETURN(0);
08200 END;
08300
08400 BOOLEAN PROCEDURE UPDAT(SAFE REAL ARRAY T);
08500 BEGIN"MARK3"
08600 SAFE OWN REAL ARRAY V1,V2,V3[1:4];
08700 LABEL L1;
08800 INTEGER I,C,D,B,PR;
08900 REAL R;
09000 STRING S2,S3;
09100 PUSH_FORMAT(5,1);
09200 L1: S3←NULL;
09300 FOR I←1 STEP 1 UNTIL 3 DO S3←S3&CVF(VA[I])&(IF I<3 THEN"," ELSE"; ");
09400 FOR I←1 STEP 1 UNTIL 3 DO S3←S3&CVF(VO[I])&(IF I<3 THEN"," ELSE"; ");
09500 OUTSTR(S3&'15&'12);
09600 S3←INCHWL;
09700 IF LENGTH(S3) THEN BEGIN
09800 I←1;
09900 REPLACE(PR);
10000 WHILE LENGTH(S3) DO BEGIN
10100 S2←SCAN(S3,1,B);
10200 R←REALSCAN(S2,B);
10300 IF B≠-1 THEN IF I>3 THEN VO[I-3]←R ELSE VA[I]←R;
10400 I←I+1;
10500 END;
10600 UNIT(VO,VO);
10700 REDUCE(VO);
10800 VA[4]←1.0;
10900 PR←ABLE(VA,VO,T);
11000 OUTSTR("RANGE "&PRINT(PR)&'15&'12);
11100 END ELSE OUTSTR(PRINT(PR)&'15&'12);
11200 ROTAT←REALSCAN((S2←INCHWL),I);
11300 ROTAT←ROTAT/RAD;
11400 POP_FORMAT;
11500 RETURN(POSSIBLE(T,TF,ROTAT));
11600 END"MARK3";
11700
11800 STRING PROCEDURE PRINTSQUARE(REAL ARRAY A;INTEGER I);
11900 BEGIN INTEGER J,K;STRING S;
12000 PUSH_FORMAT(6,2);
12100 J←I;
12200 S←NULL;
12300 FOR K←J STEP 1 UNTIL J+3 DO S←S&CVF(A[K]);
12400 S←S&"
12500 ";
12600 FOR K←J+4 STEP 1 UNTIL J+7 DO S←S&CVF(A[K]);
12700 S←S&"
12800 ";
12900 FOR K←J+8 STEP 1 UNTIL J+11 DO S←S&CVF(A[K]);
13000 S←S&"
13100 ";
13200 FOR K←J+12 STEP 1 UNTIL J+15 DO S←S&CVF(A[K]);
13300 S←S&"
13400
13500
13600 ";
13700 POP_FORMAT;
13800 RETURN (S) END;
13900
14000 BOOLEAN PROCEDURE QUERY(REFERENCE REAL R;STRING N);
14100 BEGIN STRING S;
14200 REAL T;
14300 INTEGER I;
14400 OUTSTR(N&" "&CVF(R)&" ");
14500 S←INCHWL;
14600 IF ¬LENGTH(S) THEN RETURN(FALSE);
14700 IF EQU(S,"π")THEN BEGIN R←PI;RETURN(TRUE) END;
14800 IF EQU(S,"π/2")THEN BEGIN R←PIBY2;RETURN(TRUE) END;
14900 IF EQU(S,"π/4")THEN BEGIN R←PIBY2/2;RETURN(TRUE) END;
15000 IF EQU(S,"-π/2")THEN BEGIN R←-PIBY2;RETURN(TRUE) END;
15100 IF EQU(S,"-π/4")THEN BEGIN R←-PIBY2/2;RETURN(TRUE) END;
15200 IF EQU(S,"-π") THEN BEGIN R←-PI;RETURN(TRUE) END;
15300 T←REALSCAN(S,I);
15400 R←IF I=-1 THEN R ELSE T;
15500 RETURN(I≠-1);
15600 END;
15700
00100 INTEGER FILELENGTH;
00200 BOOLEAN TABLE_COORDS;
00300 FORMAT_POINTER←-1;
00400 PUSH_FORMAT(8,4);
00500 RESET;
00600 FOR I←1 STEP 1 UNTIL 6 DO MMOVE(A[SQAR(I)],A[SQAR(I)]);
00700
00800 BREAKSET(1," ,;:","I");
00900 FILE←"ARM";
01000 MMOVE(Q[0],Q[0]);
01100 MMOVE(Q[17],Q[17]);
01200 FOR I←1 STEP 1 UNTIL 6 DO BEGIN
01300 N←SQAR(I);
01400 MMOVE(JMAT[N],JMAT[N])END ;
01500 ARM_POSITION;
01600 OUTSTR("TABLE ? ");
01700 TABLE_COORDS←INCHWL="Y";
01800 FOR I←1 STEP 1 UNTIL 6 DO
01900 DEPART[I]←ARRIVE[I]←IF I=3 ∧ TABLE_COORDS THEN 1.0 ELSE 0.0;
02000 OPEN(4,"DSK",'10,0,1,120,BREAK,EOF);
02100 OPEN(5,"DSK",'10,1,0,120,BREAK,EOF);
02200 DO BEGIN
02300 OUTSTR("FILE NAME"&'15&'12);
02400 LOOKUP(5,DATFIL←INCHWL,J);
02500 IF ¬J THEN
02600 OUTSTR("FILE EXISTS. CONCATERNATE ?");
02700 END UNTIL J ∨ INCHWL="Y";
02800 ENTER(4,DATFIL,EOF);
02900 FILELENGTH←0;
03000 IF ¬J THEN DO BEGIN
03100 ARRYIN(5,COEFF[0],1024);
03200 FOR I←0 STEP 1 UNTIL 1023 DO IF COEFF[I]=100.0 THEN DONE;
03300 ARRYOUT(4,COEFF[0],FILELENGTH←FILELENGTH+I);
03400 END UNTIL COEFF[I]=100.0;
03500 WHILE TRUE DO BEGIN
03600 IF TABLE_COORDS THEN BEGIN
03700 DO UNTIL UPDAT(TRANS);
03800 ARRYOUT(4,TF[1],6);
03900 HANDPOS(TF);
04000 FOR I←1 STEP 1 UNTIL 6 DO DIR[I]←IF I=3 THEN 0.1 ELSE 0.0;
04100 INCREMENT(DTH,DIR);
04200 UNDERFLOW(TRUE);
04300 FOR I←1 STEP 1 UNTIL 6 DO
04400 BEGIN
04500 RES[I]←0;
04600 END;
04700 FOR I←1 STEP 1 UNTIL 6 DO
04800 BEGIN FOR J←1 STEP 1 UNTIL 6 DO DIR[J]←0;
04900 DIR[I]←0.01;
05000 SOLVE(6,LU,DIR,DTH);
05100 IMPROVE(6,NR,LU,DIR,DTH,DIGITS);
05200 FOR J←1 STEP 1 UNTIL 6 DO RES[J]←RES[J]+DTH[J]↑2;
05300 END;
05400 FOR I←1 STEP 1 UNTIL 6 DO
05500 BEGIN RES[I]←SQRT(RES[I]);
05600 OUTSTR(CVF(RES[I]));
05700 END;
05800 OUTSTR('15&'12);
05900 UNDERFLOW(FALSE);
06000 END ELSE BEGIN
06100 PUSH_FORMAT(2,4);
06200 RES[1]←0.005;
06300 RES[2]←0.005;
06400 RES[3]←0.03;
06500 RES[4]←0.01;
06600 RES[5]←0.01;
06700 RES[6]←0.02;
06800 FOR I←1 STEP 1 UNTIL 6 DO
06900 IF ¬QUERY(TF[I],"THETA"&CVS(I)) THEN RES[I]←100.0;
07000 POP_FORMAT;
07100 ARRYOUT(4,TF[1],6);
07200 END;
07300 ARRYOUT(4,RES[1],6);
07400 DO BEGIN
07500 START_TRAJECTORY(CVSIX(FILE));
07600 TRAJECTORY(ARM_VECTOR,TF);
07700 CLOSE_TRAJECTORY;
07800 DO_IT(CVSIX(FILE));
07900 WHILE ARM_MOTION DO CALL(1,"SLEEP");
08000 OUTSTR(CVOS(ARM_STATUS)&'15&'12);
08100 IF ARM_STATUS THEN ARM_POSITION;
08200 END UNTIL ¬ARM_STATUS;
08300 IF INCHWL≠"B" THEN BEGIN
08400 ARM_JOINT;
08500 ARRYOUT(4,ARM_VECTOR[1],6);
08600 ARRYOUT(4,100.0,1);
08700 I←FILELENGTH←FILELENGTH+18;
08800 END ELSE I←FILELENGTH;
08900 CLOSE(5);
09000 CLOSE(4);
09100 LOOKUP(5,DATFIL,EOF);
09200 IF EOF THEN USERERR(0,1,"DATA FILE HAS GONE AWAY");
09300 ENTER(4,DATFIL,EOF);
09400 WHILE I>0 DO BEGIN
09500 ARRYIN(5,COEFF[0],1024);
09600
09700 J←IF I<1024 THEN I ELSE 1024;
09800 ARRYOUT(4,COEFF[0],J);
09900 I←I-1024;
10000 END;
10100 ARM_POSITION;
10200 ARRTRAN(TF,ARM_VECTOR);
10300 END;
10400 END;